COMMENT ⊗   VALID 00011 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	ENTRY
C00003 00003	!	sense, bias, comply
C00006 00004	!	grasp
C00007 00005	!	open_hand 
C00008 00006	!	reach
C00010 00007	!	move
C00014 00008	! MSM1:	release, get,put
C00016 00009	! MSM2:	transfer,cross_insert,guided_insert
C00019 00010	!	msmcall
C00023 00011	END "MSM"
C00024 ENDMK
C⊗;
ENTRY;
BEGIN "MSM"
COMMENT routines which are not available in AL;
DEFINE $MSM=TRUE;
REQUIRE "HEADER.SAI" SOURCE_FILE;

STRING INPUT_STRING;
STRING CURRENTFRAME;

RPTR(EXPR$) PROCEDURE $GTIDREF(INTEGER TYPE; STRING S);
	BEGIN ! like $$gtidref except does not return sym ptr;
	RPTR(SYMBOL)SYM;
	RETURN($$GTIDREF(TYPE,SYM,S));
	END;

STRING PROCEDURE CHOP$CLNSAVE;
	BEGIN STRING S;
	S←$CLNSAVE[1 TO ∞ - LENGTH(TOKEN)];
	$CLNSAVE←TOKEN;
	RETURN(S);
	END;
!	sense, bias, comply;

PRELOAD_WITH "FX","FY","FZ","TX","TY","TZ";
STRING ARRAY FTYPE[1:6];
PRELOAD_WITH "FORCE(XHAT)","FORCE(YHAT)","FORCE(ZHAT)",
		"TORQUE(XHAT)","TORQUE(YHAT)","TORQUE(ZHAT)";
STRING ARRAY FSTYPE[1:6];

RECURSIVE PROCEDURE SENSE;
BEGIN	INTEGER I; RPTR(EXPR$)E; STRING S;
	S←" ON "; $CLNSAVE←NULL;
	GTOKEN;
	FOR I←1 STEP 1 UNTIL 6 DO IF EQU(FTYPE[I],TOKEN) THEN DONE;
	IF I>6 THEN ERROR("Need FX,FY,FZ,TX,TY,TZ here");
	S←S&FSTYPE[I];
	GTOKEN;
	IF TOKEN≠">" AND TOKEN≠"<" AND TOKEN≠"≥" AND TOKEN≠"≤" THEN
		ERROR("Need >,<,≥,≤ here");
	s←s&" "&TOKEN;
	$CLNSAVE←NULL;
	E←$$GTANYEXP("SENSE",#SC);
	S←S&" "&CHOP$CLNSAVE&"DO ";
	GTOKEN;
 	IF TOKENPTR≠NULL_RECORD AND SYMBOL:ACCESS[TOKENPTR]=#PROCEDURE
		THEN BEGIN PREF(TOKENPTR);
		S←S&CHOP$CLNSAVE;
		END
	ELSE IF EQU(TOKEN,"STOP")
		THEN BEGIN
			GTOKEN;
			IF EQU(TOKEN,"BARM") OR EQU(TOKEN,"YARM")
			THEN BEGIN S←S&" STOP "&TOKEN; GTOKEN; END
			ELSE S←S&" STOP "&CURRENTFRAME;
			CHOP$CLNSAVE;
			STOKEN←TRUE;
		     END
	ELSE ERROR("REQUIRE A PROCEDURE HERE");
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;

RECURSIVE PROCEDURE BIAS;
BEGIN	INTEGER I; RPTR(EXPR$)E;
	STRING S;
	S←" WITH "; $CLNSAVE←NULL;
	GTOKEN;
	FOR I←1 STEP 1 UNTIL 6 DO IF EQU(FTYPE[I],TOKEN) THEN DONE;
	IF I>6 THEN ERROR("Need FX,FY,FZ,TX,TY,TZ here");
	S←S&FSTYPE[I];
	GTOKEN;
	IF TOKEN≠"=" THEN ERROR("Need = here");
	s←s&" "&TOKEN;
	$CLNSAVE←NULL;
	E←$$GTANYEXP("SENSE",#SC);
	S←S&" "&CHOP$CLNSAVE&" ";
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;

RECURSIVE PROCEDURE COMPLY;
BEGIN	
	STRING S; RPTR(EXPR$)E;
	S←" WITH STIFFNESS=(";
	$CLNSAVE←NULL;
	E←$$GTANYEXP("COMPLY",#VT);
	S←S&" "&CHOP$CLNSAVE&",";
	E←$$GTANYEXP("COMPLY",#VT);
	S←S&CHOP$CLNSAVE&") AT ";
	E←$$GTANYEXP("COMPLY",#TR);
	S←S&CHOP$CLNSAVE&" ";
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;
!	grasp;
PROCEDURE GRASP(INTEGER N);
BEGIN	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	IF N=1 THEN S←"GRASP1" ELSE IF N=2 THEN S←"GRASP2"
		ELSE ERROR("ONLY GRASP1 OR GRASP2 ALLOWED");
	S←S&"(";
	E←$$GTXP2;
	IF E=NULL_RECORD THEN S←S&"0,HAND_MAX);"
	    ELSE BEGIN
		S←S&CHOP$CLNSAVE&",";
		E←$$GTXP2;
		IF E=NULL_RECORD THEN S←S&"HAND_MAX);"
			ELSE S←S&CHOP$CLNSAVE&");";
		END;
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;
!	open_hand ;
RECURSIVE PROCEDURE OPEN_HAND;
BEGIN	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"OPEN_HAND";
	GTOKEN; IF TOKEN="+" THEN S←S&"P" ELSE STOKEN←TRUE;
	S←S&"(";
	E←$$GTXP2;
	IF E THEN S←S&CHOP$CLNSAVE&");"
	     ELSE S←S&"HAND_MAX);";
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;
!	reach;

!	bit position values:
for reach and moves:
	MM=3,MS=2,SM=1,SS=0, and F = add 4 ;

PRELOAD_WITH "REACHSS","REACHSM","REACHMS","REACHMM";
STRING ARRAY REACHNAME[0:4];
PRELOAD_WITH ";",NULL,";",NULL;
STRING ARRAY DEFAULT_END[0:4];
PRELOAD_WITH "PARK_POSITION","PARK_POSITION","PARK_POSITION","PARK_POSITION";
STRING ARRAY DEFAULT_DEST[0:4];
PRELOAD_WITH "DTOL","DTOL","DTOL","1000*INCHES";
STRING ARRAY DEFAULT_TOL[0:4];
PRELOAD_WITH "RTOL","RTOL","RTOL","RTOL";
STRING ARRAY DEFAULT_ATOL[0:4];

RECURSIVE PROCEDURE REACH(INTEGER N);
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←REACHNAME[N];
	GTOKEN; IF TOKEN="+" THEN
		BEGIN S←S&"P"; $CLNSAVE←NULL; END
		ELSE STOKEN←TRUE;
	S←S&"(";
	E←$$GTXP2;
	IF E=NULL_RECORD THEN S←S&DEFAULT_DEST[N]&","&DEFAULT_TOL[N]&","
			&DEFAULT_ATOL[N]&")"
	    ELSE BEGIN
		S←S&"⊂"&CHOP$CLNSAVE&"⊃,";
		E←$$GTXP2;
		IF E=NULL_RECORD THEN S←S&DEFAULT_TOL[N]&","&DEFAULT_ATOL[N]&")"
		    ELSE BEGIN
			S←S&"⊂"&CHOP$CLNSAVE&"⊃,";
			E←$$GTXP2;
			IF E=NULL_RECORD THEN S←S&DEFAULT_ATOL[N]&")"
			    ELSE S←S&"⊂"&CHOP$CLNSAVE&"⊃)";
			END;
		END;
	INPUT_STRING←INPUT_STRING&CRLF&S&DEFAULT_END[N];
END;
!	move;

PRELOAD_WITH "MOVESS","MOVESM","MOVEMS","MOVEMM","FMOVESS","FMOVESM","FMOVEMS","FMOVEMM";
STRING ARRAY MOVENAME[0:7];
PRELOAD_WITH ";"," ",";"," "," "," "," "," ";
STRING ARRAY MOVEDEFAULT_END[0:7];
PRELOAD_WITH "PARK_POSITION","PARK_POSITION","PARK_POSITION","PARK_POSITION",
	     "PARK_POSITION","PARK_POSITION","PARK_POSITION","PARK_POSITION";
STRING ARRAY MOVEDEFAULT_DEST[0:7];
PRELOAD_WITH "DTOL","DTOL","DTOL","1000*INCHES","DTOL","DTOL","DTOL","DTOL";
STRING ARRAY MOVEDEFAULT_TOL[0:7];
PRELOAD_WITH "RTOL","RTOL","RTOL","RTOL","RTOL","RTOL","RTOL","RTOL";
STRING ARRAY MOVEDEFAULT_ATOL[0:7];

RECURSIVE PROCEDURE MOVE(INTEGER TYPE);
BEGIN
CASE TYPE OF
  BEGIN
  [0] [1] [2][3]
      BEGIN ! SS , SM ;
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←MOVENAME[TYPE];
	GTOKEN; IF TOKEN="+" THEN S←S&"P" ELSE STOKEN←TRUE;
	S←S&"(";
	IF TYPE=0 OR TYPE=1 THEN
	    BEGIN
	    E←$GTIDREF(#FR,"MSM MOVE COMMAND");
	    S←S&(CURRENTFRAME←CHOP$CLNSAVE);
	    END
	ELSE S←S&CURRENTFRAME;
	E←$$GTXP2;
	IF E=NULL_RECORD THEN S←S&")"
	    ELSE BEGIN
		S←S&",⊂"&CHOP$CLNSAVE&"⊃";
		E←$$GTXP2;
		IF E=NULL_RECORD THEN S←S&")"
		    ELSE BEGIN
			S←S&",⊂"&CHOP$CLNSAVE&"⊃";
			E←$$GTXP2;
			IF E=NULL_RECORD THEN S←S&")"
			    ELSE S←S&",⊂"&CHOP$CLNSAVE&"⊃)";
			END;
		END;
	INPUT_STRING←INPUT_STRING&CRLF&S&MOVEDEFAULT_END[TYPE];
      END;
  [4][5][6][7]
      BEGIN ! FSS,FSM ;
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←MOVENAME[TYPE];
	GTOKEN; IF TOKEN="+" THEN S←S&"P" ELSE STOKEN←TRUE;
	S←S&"(";
	IF TYPE=4 OR TYPE=5 THEN
	    BEGIN
	    E←$GTIDREF(#FR,"FSM OR FSS MOVE COMMAND");
	    S←S&(CURRENTFRAME←CHOP$CLNSAVE);
	    END
	ELSE S←S&","&CURRENTFRAME;
	E←$$GTXP2;
	IF E=NULL_RECORD THEN S←S&")"
	    ELSE S←S&",⊂"&CHOP$CLNSAVE&"⊃)";
	INPUT_STRING←INPUT_STRING&CRLF&S&MOVEDEFAULT_END[TYPE];
	GTOKEN;
	WHILE EQU(TOKEN,"SENSE") OR EQU(TOKEN,"BIAS") OR EQU(TOKEN,"COMPLY")
		DO BEGIN
			IF EQU(TOKEN,"SENSE") THEN SENSE
			ELSE IF EQU(TOKEN,"BIAS") THEN BIAS
			ELSE IF EQU(TOKEN,"COMPLY") THEN COMPLY;
			GTOKEN;
		   END;
	STOKEN←TRUE;
      END
  END;
END;
! MSM1:	release, get,put;

PROCEDURE RELEASE;
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"RELEASE";
	E←$GTIDREF(#FR,"MSM RELEASE COMMAND");
	S←S&"("&CHOP$CLNSAVE;
	E←$$GTXP2;
	IF E THEN S←S&","&CHOP$CLNSAVE&");" ELSE S←S&");";
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;

PROCEDURE GET;
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"GET";
	E←$GTIDREF(#FR,"MSM GET COMMAND");
	S←S&"("&CHOP$CLNSAVE;
	E←$$GTANYEXP("MSM GET COMMAND",#SC);
	S←S&","&CHOP$CLNSAVE;
	E←$$GTXP2;
	IF E THEN S←S&","&CHOP$CLNSAVE&");" ELSE S←S&");";
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;

PROCEDURE PUT;
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"PUT";
	E←$GTIDREF(#FR,"MSM PUT COMMAND");
	S←S&"("&CHOP$CLNSAVE;
	E←$$GTANYEXP("MSM PUT COMMAND",#TR);
	S←S&","&CHOP$CLNSAVE;
	E←$$GTXP2;
	IF E THEN S←S&","&CHOP$CLNSAVE&");" ELSE S←S&");";
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;
! MSM2:	transfer,cross_insert,guided_insert;

PROCEDURE TRANSFER;
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"TRANSFER";
	E←$GTIDREF(#FR,"MSM TRANSFER COMMAND");
	S←S&"("&CHOP$CLNSAVE;
	E←$$GTANYEXP("MSM TRANSFER COMMAND",#SC);
	S←S&","&CHOP$CLNSAVE;
	E←$$GTANYEXP("MSM TRANSFER COMMAND",#TR);
	S←S&","&CHOP$CLNSAVE&");";
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;

PROCEDURE CROSS_INSERT;
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"CROSS_INSERT";
	E←$GTIDREF(#FR,"MSM CROSS_INSERT COMMAND, DESTINATION ARGUMENT");
	S←S&"("&CHOP$CLNSAVE;
	E←$$GTANYEXP("MSM CROSS_INSERT COMMAND, DIAMETER ARGUMENT",#SC);
	S←S&","&CHOP$CLNSAVE;
	E←$$GTANYEXP("MSM CROSS_INSERT COMMAND, LENGTH ARGUMENT",#SC);
	S←S&","&CHOP$CLNSAVE;
	E←$$GTANYEXP("MSM CROSS_INSERT COMMAND, OFFSET ARGUMENT",#SC);
	S←S&","&CHOP$CLNSAVE&");";
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;

PROCEDURE GUIDED_INSERT;
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"GUIDED_INSERT";
	E←$GTIDREF(#FR,"MSM GUIDED_INSERT COMMAND, DESTINATION ARGUMENT");
	S←S&"("&CHOP$CLNSAVE;
	E←$$GTANYEXP("MSM GUIDED_INSERT COMMAND, DIAMETER ARGUMENT",#SC);
	S←S&","&CHOP$CLNSAVE;
	E←$$GTANYEXP("MSM GUIDED_INSERT COMMAND, LENGTH ARGUMENT",#SC);
	S←S&","&CHOP$CLNSAVE;
	E←$$GTANYEXP("MSM GUIDED_INSERT COMMAND, OFFSET ARGUMENT",#SC);
	S←S&","&CHOP$CLNSAVE&");";
	INPUT_STRING←INPUT_STRING&CRLF&S;
END;
!	msmcall;
INTERNAL PROCEDURE MSMCALL;
BEGIN
    BOOLEAN MM,RM;
!   IF DEVICE≠DSK_X THEN ERROR("MSMCALL VALID ONLY FOR DISK INPUT AT THE MOMENT");
!   IF $COMPILE≠0 THEN ERROR("VALID ONLY AT TOP LEVEL");
    INPUT_STRING←NULL;
    $CLNSAVE←NULL;
    MM←RM←FALSE;
    GTOKEN;
    WHILE NOT EQU(TOKEN,"MSMEND") DO
	BEGIN
	IF RM THEN
	    BEGIN
	    IF EQU(TOKEN,"RMM") THEN REACH(3)
	    ELSE IF EQU(TOKEN,"RMS") THEN BEGIN REACH(2); RM←FALSE; END
	    ELSE ERROR("NEED RMS OR RMM HERE");
	    END
	ELSE IF MM THEN
	    BEGIN
	    IF EQU(TOKEN,"MMM") THEN MOVE(3)
	    ELSE IF EQU(TOKEN,"MMS") THEN BEGIN MOVE(2); MM←FALSE; END
	    ELSE IF EQU(TOKEN,"FMM") THEN MOVE(7)
	    ELSE IF EQU(TOKEN,"FMS") THEN BEGIN MOVE(6); MM←FALSE; END
	    ELSE ERROR("NEED MMM,MMS,FMM OR FMS HERE");
	    END
	ELSE IF EQU(TOKEN,"G1") THEN GRASP(1)
	ELSE IF EQU(TOKEN,"G2") THEN GRASP(2)
	ELSE IF EQU(TOKEN,"RSS") THEN REACH(0)
	ELSE IF EQU(TOKEN,"RSM") THEN BEGIN REACH(1); RM←TRUE; END
	ELSE IF EQU(TOKEN,"MSS") THEN MOVE(0)
	ELSE IF EQU(TOKEN,"MSM") THEN BEGIN MOVE(1); MM←TRUE; END
	ELSE IF EQU(TOKEN,"FSS") THEN MOVE(4)
	ELSE IF EQU(TOKEN,"FSM") THEN BEGIN MOVE(5); MM←TRUE; END
	ELSE IF EQU(TOKEN,"OPN")  THEN OPEN_HAND
	ELSE IF EQU(TOKEN,"RMM") OR EQU(TOKEN,"RMS")
		THEN ERROR("RMM,RMS can only follow a RSM or RMM")
	ELSE IF	EQU(TOKEN,"MMM") OR EQU(TOKEN,"MMS") OR
		EQU(TOKEN,"FMM") OR EQU(TOKEN,"FMS")
		THEN ERROR("MMM,MMS,FMM,FMS can only follow a MSM,MMM,FSM,FMM")
	ELSE IF EQU(TOKEN,"RELEASE") THEN RELEASE
	ELSE IF EQU(TOKEN,"GET") THEN GET
	ELSE IF EQU(TOKEN,"PUT") THEN PUT
	ELSE IF EQU(TOKEN,"TRANSFER") THEN TRANSFER
	ELSE IF EQU(TOKEN,"CROSS_INSERT") THEN CROSS_INSERT
	ELSE IF EQU(TOKEN,"GUIDED_INSERT") THEN GUIDED_INSERT
	ELSE INPUT_STRING←INPUT_STRING&$CLNSAVE;
	IF NOT STOKEN THEN $CLNSAVE←NULL;
	GTOKEN;
	END;
    SEMICOL_READ; STOKEN←TRUE;
    $CLNSAVE←NULL;
    ASKUSER(INPUT_STRING);
END;
END "MSM"